#ifdef GCC_MEGA64_AVR
	/* EEPROM routines used only with the WinAVR compiler. */
	#include <avr/eeprom.h> 
#endif

/* Scheduler include files. */
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
//robo include files
#include "LCDtasks.h"
#include "pwm.h"
#include "define.h"
#include "FreeRTOS_speed.h"
#include "FreeRTOS_adc.h"
#include "FreeRTOS_pid.h"
#include "FreeRTOS_physical.h"
#include "FreeRTOS_encoder.h"
#include "FreeRTOS_forward.h"
//prototype include
#include "robotasks.h"



#define roboHANDLER_STACK_SIZE		( configMINIMAL_STACK_SIZE + 20)
#define roboCONTROLLER_STACK_SIZE	( configMINIMAL_STACK_SIZE + 50 )//highest priority
#define roboQUEUE_LEN					2

/*we don't have to block to send. */
#define roboNO_BLOCK					( ( portTickType ) 0 )
/* The task will block on the queue for a long period. */
#define roboBLOCK_TIME					( ( portTickType ) 0xffff )

static void  vInitRobo(void);
static void vControllerTask( void *pvParameters );
static void vHandlerTask( void *pvParameters );

static xQueueHandle xComQueue;
 	//static unsigned char ucParameterToPass;
	
void vStartRoboTasks(unsigned portBASE_TYPE uxPriority){
	static xTaskHandle xHandle_Ctl;
	static xTaskHandle xHandle_Hnd;
	vInitRobo();

	portENTER_CRITICAL();
	{
		/* Create the queues used for comunication between the Controller and Handler task. */
		xComQueue = xQueueCreate( roboQUEUE_LEN, ( unsigned portBASE_TYPE ) sizeof( char *) );
	}
	portEXIT_CRITICAL();
	if(xComQueue != 0){
		xTaskCreate( vControllerTask, ( signed char * ) "ctl", roboCONTROLLER_STACK_SIZE,(xTaskHandle*)&xHandle_Hnd, uxPriority, &xHandle_Ctl );
		xTaskCreate( vHandlerTask, ( signed char * ) "hnd", roboHANDLER_STACK_SIZE, (xTaskHandle*)&xHandle_Ctl, uxPriority-1, &xHandle_Hnd );
	}
}
static void  vInitRobo(void){
	init_timer3();//PWM
	init_ADC();//ADC
	
	PORTG=0x1f;
	DDRG=0x07;
	PORTA=0xFF;
	DDRA=0xFF;
	
	OCR3AL=0x00;
	OCR3BL=0x00;
	OCR3CL=0x00;
	// External Interrupt(s) initialization
	// INT0: Off
	// INT1: Off
	// INT2: On
	// INT2 Mode: Rising Edge
	// INT3: On
	// INT3 Mode: Rising Edge
	// INT4: Off
	// INT5: Off
	// INT6: On
	// INT6 Mode: Rising Edge
	// INT7: On
	// INT7 Mode: Rising Edge
	EICRA=0xF0;
	EICRB=0xF0;
	EIMSK=0xCC;
	EIFR=0xCC;
}
static void vHandlerTask( void *pvParameters ){
	//Variables
	unsigned char  *pcHndBuffer,ucHnd_out[5];//{direction{left,right,forward,backward,stop} 
	//,crossline_num, distance, stand_speed}
	unsigned char	*pcCtlInput;//{dipsw,input1,input2, input3,crossline_flag}
	unsigned char 	start_flag=0;
	unsigned char _L1,_L2,_L3,_R1,_R2,_R3;
	int _stand_speed=30;
	int _speed_max=100;
	int _speed_min=0;
	int num_of_crossline=0;
	unsigned char num_of_noline=0;
	//Init variables
	ucHnd_out[0] = STOP_ROBO;//first: robo stop
	ucHnd_out[1] = 0;//crossline number
	ucHnd_out[2] = 0;//distance
	ucHnd_out[3] = 0;//stand speed
	
	pcHndBuffer = ucHnd_out;
	/* Just to stop compiler warnings. */
	( void ) pvParameters;
	
	//Send pointer for output to controller by Queue
	if( xQueueSend( xComQueue,(void *) &pcHndBuffer,roboNO_BLOCK) != pdPASS ){
		;
	}
	//Recieve pointer for intput from controller by Queue
	if( xQueueReceive( xComQueue, (void *)&pcCtlInput,roboBLOCK_TIME ) != pdPASS ){
		;
	}
	for(;;){
		//process inputs: buttons
		_L1 =  get_button(pcCtlInput[1],B_L1);
		_L2 =  get_button(pcCtlInput[1],B_L2);
		_L3 =  get_button(pcCtlInput[1],B_L3);
		_R1 =  get_button(pcCtlInput[1],B_R1);
		_R2 =  get_button(pcCtlInput[1],B_R2);
		_R3 =  get_button(pcCtlInput[1],B_R3);
		//process inputs: dipsw (16 modes)with equal buttons
		ucHnd_out[3]=_stand_speed;
		switch(pcCtlInput[0]){
			case 0:// for robo learn line black, white, set standspeed, required FRIST
				xPrintNumLCD(0,(long)pcCtlInput[1],3,0);
				if(_L1){
					ucHnd_out[0]= UPDATE_SENSOR;
				}else
					ucHnd_out[0]=STOP_ROBO;

				if (_L2)
				{
					faster_speed(&_stand_speed,_speed_max);
				}
				else
				if (_L3)
				{
					slower_speed(&_stand_speed,_speed_min);
				}
				
				
				break;
			case 1://just fwd	
				if(_L1) start_flag=1;
				if(_R1) start_flag=0;
				if(start_flag){		
					if(pcCtlInput[4]==NO_LINE)// line=0
						num_of_noline++;
					if(num_of_noline<50)
						ucHnd_out[0]=FORWARD_ROBO;	
					else{
						ucHnd_out[0]=STOP_ROBO;
						num_of_noline=0;
					}
				}
				else	{
					ucHnd_out[0]=STOP_ROBO;
					num_of_noline=0;
				}
				break;				
			case 2://TURNLEFT if crossline_flag
				if(_L1) start_flag=1;
				if(_R1) start_flag=0;
				if(start_flag){
					if(pcCtlInput[4]==NO_LINE)// line=0
						ucHnd_out[0]=STOP_ROBO;
					else if(pcCtlInput[4]==CROSS_LINE)//line>=4 bit
						{
							ucHnd_out[0]=FORWARD_E_ROBO;
							vTaskSuspend(NULL);
							ucHnd_out[0]=TURNLEFT_ROBO;

						}
					else if(pcCtlInput[4]==IS_LINE)//line [1,3]
						ucHnd_out[0]=FORWARD_ROBO;	
					}
				else ucHnd_out[0]=STOP_ROBO;
				break;
			case 3://TURNRIGHT if crossline_flag
				if(_L1) start_flag=1;
				if(_R1) start_flag=0;
				if(start_flag){
					if(pcCtlInput[4]==NO_LINE)// no line
						ucHnd_out[0]=STOP_ROBO;
					else if(pcCtlInput[4]==CROSS_LINE)//line>=4 bit
						{
							ucHnd_out[0]=FORWARD_E_ROBO;
							vTaskSuspend(NULL);
							ucHnd_out[0]=TURNRIGHT_ROBO;
						}
					else if(pcCtlInput[4]==IS_LINE)//line [1,3]
						ucHnd_out[0]=FORWARD_ROBO;		
					}
				else ucHnd_out[0]=STOP_ROBO;
				break;
			case 4://just for test pull up
				if(_L1) start_flag=1;
				if(_R1) start_flag=0;
				if(start_flag){
					
						ucHnd_out[0]=PULL_ROBO;
						start_flag=0;
					
				}else	{
					ucHnd_out[0]=STOP_ROBO;
					//num_of_noline=0;
				}
				
				xPrintNumLCD(0,(long)num_of_crossline,3,0);
				break;
			case  5:	//just for test	encoder	
				if(_L1) start_flag=1;
				if(_R1) start_flag=0;
				if(start_flag){		
						ucHnd_out[0]=FORWARD_E_ROBO;
						start_flag=0;
				}
				else	{
					ucHnd_out[0]=STOP_ROBO;
				}
				break;
			case  6:	//just for test	TURNRIGHT
				if(_L1) start_flag=1;
				if(_R1) start_flag=0;
				if(start_flag){
							ucHnd_out[0]=TURNRIGHT_ROBO;
							start_flag=0;
				}
				else ucHnd_out[0]=STOP_ROBO;
				break;
			default:
				//xPrintStrLCD(16,"dispsw=//",0);
				ucHnd_out[0]=STOP_ROBO;
				break;	
		}
		
		xPrintNumLCD(27,(long)_stand_speed,3,0);
		
		//wait controller done
		vTaskSuspend(NULL);

	}
}
static void vControllerTask( void *pvParameters ){
	//Variables
	unsigned char  	*pcCtlBuffer, ucCtl_out[5];//{dipsw,input1,input2, input3,crossline_flag}
	unsigned char	*pcHndInput;//{direction{left,right,forward,backward,stop}
	//,crossline_num, distance, stand_speed}
	//sensor
	unsigned char _V_compare[8] = {100,100,100,100,100,100,100,100};//this is pointer
	unsigned char 	ucSensors;
	int _dev=11, _old_dev=0,l=0,r=0;
	//turn left/right
	unsigned char function_left=TURNLEFT_NO;
	unsigned char function_right=TURNRIGHT_NO;
	unsigned char function_forward_e =FWD_E_NO; 
	unsigned char hold_velocity_flag=0,num=0;
	unsigned char delay_times; // delay for turnleft/right_mode
	int velctl_L,velctl_R;
	int L_velocity,R_velocity;
	int vel=4;
	int length=8000;
	unsigned char _L1,_L2,_L3,_R1,_R2,_R3;


	
	/* Just to stop compiler warnings. */
	//( void ) pvParameters;
	xTaskHandle *xHandle = (xTaskHandle*)pvParameters;
	 
	 portTickType xLastWakeTime;
	 const portTickType xFrequency = 100;

	//Init variables
	init_physical(&ucCtl_out[1],&ucCtl_out[2], &ucCtl_out[3], &ucCtl_out[0]);
	init_encoder();
	read_Vcompare(_V_compare);
	//---------------------------
	pcCtlBuffer = ucCtl_out;
	//Recieve pointer for intput from handler by Queue
	if( xQueueReceive( xComQueue, (void *)&pcHndInput,roboBLOCK_TIME ) != pdPASS ){
		return;
	}
	//Send pointer for output to handler by Queue
	if( xQueueSend( xComQueue,(void *) &pcCtlBuffer,roboNO_BLOCK) != pdPASS ){
		return;
	}
	//---------------------------
	// Initialise the xLastWakeTime variable with the current time.
	xLastWakeTime = xTaskGetTickCount ();
	for(;;){
		// Wait for the next cycle.
		 vTaskDelayUntil( &xLastWakeTime, xFrequency );
		 
		//update inputs: dipsw, button (IN_1,IN_2,IN_3)
		updateInput(&ucCtl_out[1],&ucCtl_out[2], &ucCtl_out[3], &ucCtl_out[0]);
		//read sonsor, check cross line: crossline_flag
		read_sensor(&ucSensors,_V_compare);

		writeIO(OUTLED,ucSensors);

		check_crossline(ucSensors,&ucCtl_out[4]);// flag crossline			
		//sample vantoc_L/R
		GET_VELOCITY(L_velocity,R_velocity);// ~ Sample_LR();
		if(hold_velocity_flag == 1)
			hold_velocity(velctl_L,velctl_R, L_velocity,R_velocity);
		//Here for delay times
		if(delay_times > 0) delay_times--;
		
		//get for FORWARD_E_ROBO
		if(num==3){
			xPrintNumLCD(0,(long)vel,2,0);
			xPrintNumLCD(4,(long)length,5,0);
			_L2 =  get_button(ucCtl_out[1],B_L2);
			_L3 =  get_button(ucCtl_out[1],B_L3);
			_R2 =  get_button(ucCtl_out[1],B_R2);
			_R3 =  get_button(ucCtl_out[1],B_R3);
			if(_L2) vel++;
			if(_L3) vel--;
			if(_R2) length +=50;
			if(_R3) length -=50;
			num=0;
		}else num++;
		//
		
		xPrintNumLCD(12,(long)pcHndInput[0],2,0);
		switch(pcHndInput[0]){
			case	STOP_ROBO:
				speed(0,0);
				vTaskResume(*xHandle);
				break;
			case	FORWARD_ROBO:
				//forward_mode()				
				xPrintNumLCD(0,(long)l,3,0);
				xPrintNumLCD(7,(long)r,3,0);
				test_sensor(ucSensors, &_dev, &_old_dev);
				calc_forward_pd(_dev, &_old_dev,pcHndInput[3],&l,&r);

				vTaskResume(*xHandle);
				break;
			case	FORWARD_E_ROBO://8000
				xPrintNumLCD(16,(long)R_ecdr_sbuf,4,0);
				xPrintNumLCD(21,(long)L_ecdr_sbuf,4,0);
				if(function_forward_e == FWD_E_DONE ){
					function_forward_e = FWD_E_NO;
					//wake up handler here
					vTaskResume(*xHandle);
				}
				else if(function_forward_e == FWD_E_NO)
					function_forward_e = FWD_E_START;
				else{
					
					run_forward_vel_encoder(&function_forward_e,length,
								&velctl_L, &velctl_R, &hold_velocity_flag,vel);
					}				
				break;
			case	BACKWARD_ROBO:
				//
				vTaskResume(*xHandle);
				break;
				
			case	TURNRIGHT_ROBO:
				if(function_right== TURNRIGHT_DONE){
					function_right = TURNRIGHT_NO;
					//wake up handler here
					vTaskResume(*xHandle);

				}
				else if(function_right == TURNRIGHT_NO){
					function_right = TURNRIGHT_START;
					
					}
				else
					turnright_process(&function_right, &velctl_L, &velctl_R, &hold_velocity_flag ,&delay_times);
				break;
			case	TURNLEFT_ROBO:
				if(function_left == TURNLEFT_DONE){
					function_left = TURNLEFT_NO;
					//wake up handler here
					vTaskResume(*xHandle);
				}
				else if(function_left == TURNLEFT_NO)
					function_left = TURNLEFT_START;
				else turnleft_process(&function_left, &velctl_L, &velctl_R, &hold_velocity_flag ,&delay_times);
				
				break;
			case 	PULL_ROBO:
				speed(0,0);
				if(get_sensor(ucCtl_out[2],B_SENSOR_HIGH)){
						dc3_speed(50);
				}
				else{
					dc3_speed(0);
					vTaskResume(*xHandle);
				}
				break;
			case	UPDATE_SENSOR:
				speed(0,0);
				save_sensor(BLACK,_V_compare);
				xPrintStrLCD(16,"Up",0);
				vTaskResume(*xHandle);
			default:
				vTaskResume(*xHandle);
				break;
		}
		
		
	}
}
